#include "encoder.h"
#include "hw_pid.h"
#include "VL53L0x.h"
#include "stdio.h"

volatile short Encoder1_Count;
volatile short Encoder2_Count;

void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) {
    static unsigned char Timer2Count;
    static unsigned char Timer6Count;
    static float a, b;

    if (htim == &htim2) {    // htim2 30KHZ 进入一次
        Timer2Count++;
        if ((Timer2Count % 150) == 0) { // 5ms 执行一次

            Encoder1_Count = ENCODERA_GET_COUNTER();
            Encoder2_Count = ENCODERB_GET_COUNTER();
            ENCODERA_SET_COUNTER_ZERO();
            ENCODERB_SET_COUNTER_ZERO();

            // printf("Encoder1_Count = %d  Encoder2_Count = %d \r\n", Encoder1_Count, Encoder2_Count);

            MotorA_Speed = (float) (Encoder1_Count) * 200 / (float) 1040.0;
            MotorB_Speed = (float) (Encoder2_Count) * 200 / (float) 1040.0;
        }
        if ((Timer2Count % 300) == 0) { // 10ms 执行一次
            a = PID_realize(&pidA, MotorA_Speed);
            b = PID_realize(&pidB, MotorB_Speed);
            Motor_Set(a, b);
            // ANO_DT_Send_F2(MotorA_Speed * 100, MotorB_Speed * 100, a * 10, b * 10);
            Timer2Count = 0;
        }
    }

    if (htim == &htim6) { // 1khz 进入一次
        Timer6Count++;
        if ((Timer6Count % 10) == 0) {// 10ms 执行一次
            HW_GET();
        }
//        if ((Timer6Count % 100) == 0) {
//            VL53L0x_StartConversion(&hi2c3);
//            VL53L0x_ReadDistance(&hi2c3, &VL53L0x_Bottom);
//            printf(" %d %d   ---- \r\n ", VL53L0x_Bottom.distValid, VL53L0x_Bottom.dist);
//            if (VL53L0x_Bottom.dist < 150 && VL53L0x_Bottom.dist > 100) {
//                STBY_RESET();
//            } else{
//                STBY_SET();
//            }
//            Timer6Count = 0;
//        }
    }

}
